#include "pub_box.h"

ProcessBoxes::ProcessBoxes()
{
    getParam();
    cloud_.reset(new pcl::PointCloud<PointT>);
    cloud_voxel_.reset(new pcl::PointCloud<PointT>);
    
    pub_boxes_ = n_.advertise<detect_msgs::Boxes>(topic_boxes_new_, 10);

    sub_cloud_.subscribe(n_, topic_cloud_, 10);
    sub_boxes_.subscribe(n_, topic_boxes_, 10);

    sync_ = std::make_shared<Sync>(MySyncPolicy(10), sub_cloud_, sub_boxes_);
    sync_->registerCallback(boost::bind(&ProcessBoxes::callBack, this, _1, _2));
    

}

 void ProcessBoxes::callBack(const sensor_msgs::PointCloud2ConstPtr& msg_cloud, const detect_msgs::BoxesConstPtr& msg_boxes)
 {  
    auto t1 = std::chrono::high_resolution_clock::now();
    if(is_filter_)
    {
        pcl::fromROSMsg(*msg_cloud, *cloud_);  // 雷达坐标系,x为深度
        filterVoxel(cloud_, cloud_voxel_, filter_size_);
    }
    else
    {
        pcl::fromROSMsg(*msg_cloud, *cloud_voxel_);
    }

    pcl::PointCloud<PointT>::Ptr cloud_cam(new pcl::PointCloud<PointT>);
    pcl::PointCloud<PointT>::Ptr cloud_uv(new pcl::PointCloud<PointT>);

    pcl::transformPointCloud(*cloud_voxel_, *cloud_cam, Eigen::Affine3d(Tcl_)); // 点云转到相机坐标下
    // 旋转后，现在的z为原来的x,原来的x表示深度
    // pcl::transformPointCloud(*cloud_cam, *cloud_uv, Eigen::Affine3d(K_));   // 像素坐标但未归一化
    
    Eigen::Vector3d p_uv(0,0,1);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>);
    
    pcl::PointXYZ p;
    cloud1->points.clear();
    for(auto point : cloud_cam->points)
    {
        if(point.z < 0 || point.z > 50.0)   // 只处理相机视角下正方向的值,和大于50m深度过滤掉
            continue;

        // (x,y,z)
        // (u-cx)
        p_uv[0] = point.x * fx_ * 1.0 / point.z + cx_;
        p_uv[1] = point.y * fy_ * 1.0 / point.z + cy_;

        if(p_uv[0] >= 0 && p_uv[0] <= 1280 &&
        p_uv[1] >= 0 && p_uv[1] <= 480)
        {   
            p.x = int(p_uv[0]);
            p.y = int(p_uv[1]);
            p.z = point.z;
            cloud1->points.emplace_back(p);

           
        }
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZ>);
    
    msg_boxes_.Boxes.clear();
    cloud2->points.clear();
    if(msg_boxes->Boxes.size())
    {   
        for (auto it : msg_boxes->Boxes)
        {
            *cloud2 = * cloud1;
            cv::Rect box = cv::Rect(it.x, it.y, it.w, it.y); // msg中box已经恢复尺度了
            passThroughFilter(cloud2, box);

            if(!cloud2->points.size())
                continue;
            
            Eigen::Vector3d cloud_min(0,0,50);
    
            for (auto point : cloud2->points)
            {
                if(point.z < cloud_min[2])
                    cloud_min = Eigen::Vector3d(point.x, point.y, point.z);
            }

            float x =  (cloud_min[0] - cx_)  * cloud_min[2] / fx_;
            float y =  (cloud_min[1] - cy_)  * cloud_min[2] / fy_;

            msg_box_.class_name = it.class_name;
            msg_box_.id = it.id;
            msg_box_.score = it.score;
            msg_box_.x = box.x;
            msg_box_.y = box.y;
            msg_box_.w = box.width;
            msg_box_.h = box.height;
            msg_box_.p_z = cloud_min[2];
            msg_box_.p_x = x;
            msg_box_.p_y = y; 

            msg_boxes_.Boxes.emplace_back(msg_box_);

            std::cout << msg_box_.class_name << " " 
                << msg_box_.x << " " << msg_box_.y << " " << msg_box_.w << " " << msg_box_.h << " "
                << msg_box_.p_x << " " << msg_box_.p_y << " " << msg_box_.p_z << " "
                << msg_box_.score << std::endl;
        }
    }
    auto t2 = std::chrono::high_resolution_clock::now();
    auto cost_time = std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1);
    
    pub_boxes_.publish(msg_boxes_);  // 发布检测结果
    ROS_INFO("Ranging completed %.2f fps", 1000. / cost_time.count()  );

 }

void ProcessBoxes::filterVoxel(const pcl::PointCloud<PointT>::Ptr& cloud_in, pcl::PointCloud<PointT>::Ptr& cloud_out, float size)
{
    pcl::VoxelGrid<PointT> sor; // 创建体素滤波器
    sor.setInputCloud(cloud_in);          // 输入点云
    // sor.setLeafSize(0.01f, 0.01f, 0.01f); //设置滤波时创建的体素体积为1cm的立方体,m
    sor.setLeafSize(size, size, size);
    sor.filter(*cloud_out);
}

void ProcessBoxes::passThroughFilter( pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_in, const cv::Rect& box)
{   
    pcl::PointCloud<pcl::PointXYZ> cloud_x, cloud_y;
    pcl::PassThrough<pcl::PointXYZ> pass_x;  // 创建直通滤波器对象
    pass_x.setInputCloud(cloud_in);            // 设置输入对象,传地址
    pass_x.setFilterFieldName("x");             // 设置过滤时所需要点云类型的X字段
    pass_x.setFilterLimits(box.x, box.x + box.width);   
    pass_x.filter(*cloud_in);     
    
    pcl::PassThrough<pcl::PointXYZ> pass_y; 
    pass_y.setInputCloud(cloud_in);  
    pass_y.setFilterFieldName("y");             // 设置过滤时所需要点云类型的Y字段
    pass_y.setFilterLimits(box.y, box.y + box.height );   
    pass_y.filter(*cloud_in);

    // pcl::PassThrough<pcl::PointXYZRGB> pass_z; 
    // pass_z.setInputCloud(cloud_in);  
    // pass_z.setFilterFieldName("z");             // 设置过滤时所需要点云类型的Z字段
    // pass_z.setFilterLimits(0.1, 30);           // 设置过滤字段的范围.  防止0
    // pass_z.filter(*cloud_in);
}

void ProcessBoxes::Rt2T(Eigen::Isometry3d& T)
{
    // R,t->Tcl 
    Eigen::Vector3d R_euler(roll_, pitch_, yaw_); // 旋转
    R_euler = R_euler / 180 * M_PI;
    
    Eigen::Vector3d t(x_, y_, z_);  // 平移
    Eigen::Matrix3d R;
    R = Eigen::AngleAxisd(R_euler[0], Eigen::Vector3d::UnitZ()) * 
        Eigen::AngleAxisd(R_euler[1], Eigen::Vector3d::UnitY()) * 
        Eigen::AngleAxisd(R_euler[2], Eigen::Vector3d::UnitX());
    T.rotate(R);
    T.pretranslate (t);
}

void ProcessBoxes::getParam()
{
    n_.getParam("topic_boxes", topic_boxes_);
    n_.param<std::string>("topic_boxes", topic_boxes_, "/detect/boxes");
    
    n_.getParam("topic_cloud", topic_cloud_);
    n_.param<std::string>("topic_cloud", topic_cloud_, "/velodyne_points");

    n_.getParam("topic_boxes_new", topic_boxes_new_);
    n_.param<std::string>("topic_boxes_new", topic_boxes_new_, "/detect/boxes_new");

    n_.getParam("filter_size", filter_size_);
    n_.param<float>("filter_size",filter_size_, 0.1);

     n_.getParam("is_filter", is_filter_);
    n_.param<bool>("is_filter", is_filter_, false);

    // -------------内参-----------------------
    n_.getParam("fx", fx_);
    n_.param<float>("fx", fx_, 718.856); 
    n_.getParam("fy", fy_);
    n_.param<float>("fy", fy_, 718.856); 
    n_.getParam("cx", cx_);
    n_.param<float>("cx", cx_, 607.1928); 
    n_.getParam("cy", cy_);
    n_.param<float>("cy", cy_, 185.2157); 
    
    n_.getParam("k1", k1_);
    n_.param<float>("k1", k1_, 0.0); 
    n_.getParam("k2", k2_);
    n_.param<float>("k2", k2_, 0.0); 
    n_.getParam("k3", k3_);
    n_.param<float>("k3", k3_, 0.0); 
    
    n_.getParam("p1", p1_);
    n_.param<float>("p1", p1_, 0.0); 
    n_.getParam("p2", p2_);
    n_.param<float>("p2", p2_, 0.0); 

    //  --------Tcl--------
    n_.getParam("roll", roll_);
    n_.param<float>("roll", roll_, 90.); 
    n_.getParam("pitch_", pitch_);
    n_.param<float>("pitch", pitch_, -90.); 
    n_.getParam("yaw", yaw_);
    n_.param<float>("yaw", yaw_, 0.0); 
    
    n_.getParam("x", x_);
    n_.getParam("y", y_);
    n_.getParam("z", z_);

    n_.param<float>("x", x_, -1.198459927713e-02);  
    n_.param<float>("y", y_, 5.403984729748e-02); 
    n_.param<float>("z", z_, -2.921968648686e-01); 

    Rt2T(Tcl_);
    std::cout << "\n --T_lidar2cam:\n" << Tcl_.matrix() << std::endl;
    std::cout << "\n-------------------\n" << std::endl;
    
    std::cout << " --filter_size : "<< filter_size_ << std::endl;
    std::cout << " --is_filter: " << is_filter_ << std::endl; 
    std::cout << " --topic_cloud: " << topic_cloud_ << std::endl; 
    std::cout << " --topic_boxes: " << topic_boxes_ << std::endl; 
    std::cout << " --topic_boxes_new: " << topic_boxes_new_ << std::endl; 

    std::cout << "\n---------------\n" << std::endl;

}
